{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Scenario Class Hierarchy\n",
    "\n",
    "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/Scenario.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
    "\n",
    "## Overview\n",
    "\n",
    "The `Scenario` class hierarchy provides a simple way to define theoretical trajectories for testing navigation algorithms and factors, particularly IMU factors. \n",
    "\n",
    "The base `Scenario` class is abstract and defines an interface that requires subclasses to provide the ground truth pose, angular velocity (in body frame), linear velocity (in nav frame), and linear acceleration (in nav frame) at any given continuous time `t`.\n",
    "\n",
    "Concrete subclasses like `ConstantTwistScenario` and `AcceleratingScenario` implement specific types of motion."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [],
   "source": [
    "%pip install --quiet gtsam-develop"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Base Class: `Scenario`\n",
    "\n",
    "Defines the required interface for any trajectory scenario.\n",
    "\n",
    "**Pure Virtual Methods (must be implemented by subclasses):**\n",
    "- `pose(t)`: Returns the `Pose3` of the body in the navigation frame at time `t`.\n",
    "- `omega_b(t)`: Returns the angular velocity `Vector3` in the **body frame** at time `t`.\n",
    "- `velocity_n(t)`: Returns the linear velocity `Vector3` in the **navigation frame** at time `t`.\n",
    "- `acceleration_n(t)`: Returns the linear acceleration `Vector3` in the **navigation frame** at time `t`.\n",
    "\n",
    "**Derived Methods (provided by the base class):**\n",
    "- `rotation(t)`: Returns the `Rot3` part of `pose(t)`.\n",
    "- `navState(t)`: Returns the `NavState` (Pose + Nav Velocity) at time `t`.\n",
    "- `velocity_b(t)`: Calculates linear velocity in the **body frame** using $v_b = R_{bn} v_n$.\n",
    "- `acceleration_b(t)`: Calculates linear acceleration in the **body frame** using $a_b = R_{bn} a_n$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Concrete Scenarios\n",
    "\n",
    "### `ConstantTwistScenario`\n",
    "\n",
    "- **Description**: Models motion with a constant *twist* (angular velocity $\\omega_b$ and linear velocity $v_b$) defined in the **body frame**.\n",
    "- **Motion**: Results in helical or circular motion trajectories.\n",
    "- **Constructors**:\n",
    "  - `ConstantTwistScenario(omega_b, velocity_b, initial_nTb=Pose3())`\n",
    "  - `ConstantTwistScenario(twist_vector_6d, initial_nTb=Pose3())`\n",
    "- **Key Behavior**: `omega_b(t)` is constant. `velocity_n(t)` and `acceleration_n(t)` are calculated based on the constant body-frame velocities and the changing orientation.\n",
    "\n",
    "### `AcceleratingScenario`\n",
    "\n",
    "- **Description**: Models motion with constant linear acceleration $a_n$ defined in the **navigation frame** and constant angular velocity $\\omega_b$ defined in the **body frame**.\n",
    "- **Motion**: Represents scenarios like accelerating along a straight line while potentially rotating (e.g., aircraft takeoff, car acceleration).\n",
    "- **Constructor**: `AcceleratingScenario(initial_nRb, initial_p_n, initial_v_n, const_a_n, const_omega_b=zero)`\n",
    "- **Key Behavior**: `acceleration_n(t)` and `omega_b(t)` are constant. `velocity_n(t)` and `pose(t)` are integrated from the initial conditions and constant rates."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Usage Example\n",
    "\n",
    "Instantiate a scenario and query its properties at different times."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "--- Circle Scenario ---\n",
      "Pose at t=1.0:\n",
      "R: [\n",
      "\t0.995004, -0.0998334, 0;\n",
      "\t0.0998334, 0.995004, 0;\n",
      "\t0, 0, 1\n",
      "]\n",
      "t:   1.99667 0.0999167         0\n",
      "Nav Velocity at t=1.0: [1.99000833 0.19966683 0.        ]\n",
      "Body Omega at t=1.0: [0.  0.  0.1]\n",
      "Nav Acceleration at t=1.0: [-0.01996668  0.19900083  0.        ]\n",
      "\n",
      "--- Accelerating Scenario ---\n",
      "Pose at t=2.0:\n",
      "R: [\n",
      "\t1, 0, 0;\n",
      "\t0, 1, 0;\n",
      "\t0, 0, 1\n",
      "]\n",
      "t: 1 0 0\n",
      "Nav Velocity at t=2.0: [1. 0. 0.]\n",
      "Nav Acceleration at t=2.0: [0.5 0.  0. ]\n"
     ]
    }
   ],
   "source": [
    "import gtsam\n",
    "import numpy as np\n",
    "\n",
    "# --- ConstantTwistScenario Example: Moving in a Circle --- \n",
    "# Forward velocity 2 m/s, turning left (positive Z rot) at ~5.73 deg/s (0.1 rad/s)\n",
    "omega_b_circle = np.array([0.0, 0.0, 0.1]) \n",
    "vel_b_circle = np.array([2.0, 0.0, 0.0]) # Forward velocity along body X\n",
    "initial_pose_circle = gtsam.Pose3() # Start at origin\n",
    "\n",
    "circle_scenario = gtsam.ConstantTwistScenario(omega_b_circle, vel_b_circle, initial_pose_circle)\n",
    "\n",
    "print(\"--- Circle Scenario ---\")\n",
    "time_t = 1.0 # seconds\n",
    "print(f\"Pose at t={time_t}:\")\n",
    "circle_scenario.pose(time_t).print()\n",
    "print(f\"Nav Velocity at t={time_t}: {circle_scenario.velocity_n(time_t)}\")\n",
    "print(f\"Body Omega at t={time_t}: {circle_scenario.omega_b(time_t)}\")\n",
    "print(f\"Nav Acceleration at t={time_t}: {circle_scenario.acceleration_n(time_t)}\") # Centripetal\n",
    "\n",
    "# --- AcceleratingScenario Example: Accelerating Straight --- \n",
    "initial_pose_accel = gtsam.Pose3() \n",
    "initial_vel_n = np.array([0.0, 0.0, 0.0])\n",
    "const_accel_n = np.array([0.5, 0.0, 0.0]) # Accelerate along nav X\n",
    "const_omega_b = np.array([0.0, 0.0, 0.0]) # No rotation\n",
    "\n",
    "accel_scenario = gtsam.AcceleratingScenario(\n",
    "    initial_pose_accel.rotation(), initial_pose_accel.translation(),\n",
    "    initial_vel_n, const_accel_n, const_omega_b\n",
    ")\n",
    "\n",
    "print(\"\\n--- Accelerating Scenario ---\")\n",
    "time_t = 2.0 # seconds\n",
    "print(f\"Pose at t={time_t}:\")\n",
    "accel_scenario.pose(time_t).print()\n",
    "print(f\"Nav Velocity at t={time_t}: {accel_scenario.velocity_n(time_t)}\")\n",
    "print(f\"Nav Acceleration at t={time_t}: {accel_scenario.acceleration_n(time_t)}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "These `Scenario` objects are primarily used as input to the `ScenarioRunner` class for generating simulated IMU data."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Source\n",
    "- [Scenario.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/Scenario.h)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py312",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
